/*
 *  VortSystem.cpp
 *  VortPan
 *
 *  Created by Gregor Veble on 7/14/10.
 *  Copyright 2010  . All rights reserved.
 *
 */

#include <iostream>
using namespace std;

#include "VortSystem.h"


using namespace VortPan;

VortSystem::VortSystem(VortMesh& InitMesh) : Mesh(InitMesh), Symmetry(false), 
MeshChecked(false), SystemSize(0), FarVelocity(e1()) {
		
}

VortSystem::~VortSystem(){
}

VortMesh& VortSystem::GetMesh(){
	return Mesh;
}



void VortSystem::AddSymmetry(VortSymmetryPlane Plane){
	Symmetry=true;
	SymmetryPlanes.push_back(Plane);
}

int VortSystem::CheckMesh(re Eps){
		
	Mesh.Bake();

	
	//Mesh.BuildEdges();
	
	int res=Mesh.CheckEdges(Symmetry, Eps, SymmetryPlanes);
	
	if (res==0) {
		MeshChecked=true;
	}
	
	Mesh.LabelSolids();
	
	Mesh.RecalculateCenters(-Eps);
	

	return res;
}

void VortSystem::ReflectPositions(Vector3D Original, PositionVector& Reflections, PositionVector& Temp){
	ReflectedPos RP,RP1;
	RP.Position=Original;
	RP.Orientation=IdentityMatrix();
	RP.Parity=true;
	
	Reflections.clear();
	Reflections.push_back(RP);
	
	for (SymmetryPlaneVector::iterator it=SymmetryPlanes.begin(); it!=SymmetryPlanes.end(); it++) {
		Temp.clear();
		for (PositionVector::iterator itp=Reflections.begin(); itp!=Reflections.end(); itp++) {
			RP=*itp;
			RP1.Position=it->Reflect(RP.Position);
			RP1.Parity=!RP.Parity;
			RP1.Orientation=RP.Orientation*it->GetReflectionMatrix(); //!!!order???
			Temp.push_back(RP);
			Temp.push_back(RP1);
		}
		Reflections=Temp;
	}
}

#define INVPI4 0.079577471545948

re VortSystem::GetSymmetrizedSolidAngleInfluence(VortPolygon& Polygon, const Vector3D& Position, bool SelfInteraction){
	PositionVector Reflections,Temp;
	re res=0;
	

	ReflectPositions(Position, Reflections, Temp);
	
	PositionVector::iterator it=Reflections.begin();
	if (SelfInteraction) {
		res=Polygon.GetPositiveSelfSolidAngle();
	} else {
		res=Polygon.GetSolidAngle(it->Position);
	}
	it++;
	
	for ( ; it!=Reflections.end(); it++) {
		res+=Polygon.GetSolidAngle(it->Position);
	}
	
	return res*INVPI4;
}

Vector3D VortSystem::GetSymmetrizedVelocityLatticeInfluence(VortPolygon& Polygon, const Vector3D& Position, re InvCoreSize2){
	PositionVector Reflections,Temp;
	Vector3D res=V0();
	
	
	ReflectPositions(Position, Reflections, Temp);
	
	
	for (PositionVector::iterator it=Reflections.begin(); it!=Reflections.end(); it++) {
		res=res+it->Orientation*Polygon.GetRegularizedVelocityInfluence(it->Position, InvCoreSize2);
	}
	
	return res;
}

re VortSystem::PolyToPolyMatrixElement(VortPolygon& PolySource, VortPolygon& PolyTarget){
	bool Self=false;
	if (&PolySource==&PolyTarget) {
		Self=true;
	}
	return GetSymmetrizedSolidAngleInfluence(PolySource, PolyTarget.GetCenterPosition(), Self);
}

re VortSystem::WakeToPolyMatrixElement(VortPolygon& PolySource, VortPolygon& PolyTarget){
	return GetSymmetrizedSolidAngleInfluence(PolySource, PolyTarget.GetCenterPosition(), false);
}

re VortSystem::PolyToWakeMatrixElement(VortPolygon& PolySource, VortPolygon& PolyTarget, bool Sign){
	if (Sign) {
		return 1;
	} else {
		return -1;
	}

}
																					
re VortSystem::WakeToWakeMatrixElement(VortPolygon& PolySource, VortPolygon& PolyTarget){
	if(&PolySource==&PolyTarget){
		return 1;
	} else {
		return 0;
	}
}


int VortSystem::AssignSpace(){
	assert(MeshChecked);
	
	SystemSize=Mesh.PolygonStorage.size()+Mesh.NSolids;
	
	Matrix.assign(SystemSize*SystemSize,0);
	RHS.assign(SystemSize,0);
	DipoleStrength.assign(SystemSize,0);
	Pivot.assign(SystemSize,0);
	
	return 0;
}

int VortSystem::MInd(int i, int j){
	return j*SystemSize+i;
}

re VortSystem::FreePotential(Vector3D Pos){
	return FarVelocity*Pos;
}

void VortSystem::SetVelocity(Vector3D Vel){
	FarVelocity=Vel;
}


void VortSystem::BuildRHS(){
	assert(MeshChecked);
	
	RHS.assign(RHS.size(),0);
	
	int NPoly=Mesh.PolygonStorage.size();
	
	for (int i=0; i<NPoly; i++) {
		VortPolygon& Poly=Mesh.PolygonStorage[i];
		if (!Poly.WakePoly) {
			RHS[i]=-FreePotential(Poly.GetCenterPosition());
		}
	}	
}

void VortSystem::BuildMatrix(){
	
	assert(MeshChecked);

	int NPoly=Mesh.PolygonStorage.size();
	
	Matrix.assign(Matrix.size(),0);
	
	for (int i=0; i<NPoly; i++) {
		VortPolygon& Target=Mesh.PolygonStorage[i];
		if (!Target.WakePoly) {
			for (int j=0; j<NPoly; j++) {
				VortPolygon& Source=Mesh.PolygonStorage[j];	
				if(!Source.WakePoly){
					re res=PolyToPolyMatrixElement(Source, Target);
					Matrix[MInd(i, j)]=res;
				}
			}
		} 
	}
	
	
	for (int i=0; i<NPoly; i++) {
		VortPolygon& Target=Mesh.PolygonStorage[i];
		if (!Target.WakePoly) {
			for (int j=0; j<NPoly; j++) {
				VortPolygon& Source=Mesh.PolygonStorage[j];	
				if(Source.WakePoly){
					Matrix[MInd(i, j)]=WakeToPolyMatrixElement(Source, Target);	
				}
			}
		} 
	}
	
	
	for (int i=0; i<NPoly; i++) {
		VortPolygon& Target=Mesh.PolygonStorage[i];
		if (Target.WakePoly) {
			Matrix[MInd(i, i)]=WakeToWakeMatrixElement(Target, Target);	

/*			for (int j=0; j<NPoly; j++) {
				VortPolygon& Source=Mesh.PolygonStorage[j];	
				if(Source.WakePoly){
					Matrix[MInd(i, j)]=WakeToWakeMatrixElement(Source, Target);	
				}
			}*/
		} 
	}
	
	for (EdgeMap::iterator it=Mesh.EdgeStorage.begin(); it!=Mesh.EdgeStorage.end(); it++) {
		VortEdgeInfo& EdgeInf=(it->second);
		if (EdgeInf.PolygonReferences.size()==3) {
			bool WakeDir;
			int wi=-1;
			int mi,mj;
			VortPolygon* pWake;

			for (int i=0; i<3; i++) {
				VortPolygonReference& PolyRef=EdgeInf.PolygonReferences[i];
				if (PolyRef.pPolygon->WakePoly) {
					wi=i;
					mi=PolyRef.Index;
					WakeDir=PolyRef.EdgeDirection;
					pWake=PolyRef.pPolygon;
				}
			}
			assert(wi>=0);
			for (int i=1; i<3; i++) {
				VortPolygonReference& PolyRef=EdgeInf.PolygonReferences[(wi+i)%3];
				assert(!PolyRef.pPolygon->WakePoly);
				bool Match=!(WakeDir!=PolyRef.EdgeDirection);
				mj=PolyRef.Index;
				Matrix[MInd(mi,mj)]=PolyToWakeMatrixElement(*(PolyRef.pPolygon), *pWake, Match);
			}
		}
	}
	
	for (int i=0; i<Mesh.NSolids; i++) {
		int ii=NPoly+i;
		for (int j=0; j<NPoly; j++) {
			VortPolygon& Poly=Mesh.PolygonStorage[j];
			if ((!Poly.WakePoly)&&(Poly.SolidIndex==i)) {
				Matrix[MInd(ii, j)]=1;
				Matrix[MInd(j, ii)]=1;
			}
		}
	}
}


void VortSystem::DecomposeMatrix(){
	lapackint Size=SystemSize;
	lapackint Info;
	
	dgetrf_(&Size, &Size, &Matrix[0], &Size, &Pivot[0], &Info);
//	cout<<"Info="<<Info<<endl;
	assert(Info==0);
}

void VortSystem::Solve(Memory& Vec){
	lapackint Size=SystemSize;
	lapackint Info;
	lapackint One=1;
	char trans='N';
		
	
	dgetrs_(&trans, &Size, &One, &Matrix[0], &Size, &Pivot[0], &Vec[0], &Size, &Info );
	
	
//	cout<<"Info="<<0<<endl;
	assert(Info==0);
	
}

re VortSystem::Potential(Vector3D Pos, Memory& Dipoles){
	int Size=Mesh.PolygonStorage.size();

	re res=FreePotential(Pos);
	
	for (int i=0; i<Size; i++) {
		re q=DipoleStrength[i];
		res+=q*GetSymmetrizedSolidAngleInfluence(Mesh.PolygonStorage[i], Pos, false);
	}
	
	return res;
}

re CapPotDiff(re x){
	if (x<-PI2) {
		x+=TWO*PI2;
	}
	if (x>PI2) {
		x-=TWO*PI2;
	}
	return x;	
}


Vector3D VortSystem::FreeVelocity(Vector3D Pos){
	return FarVelocity;
}

Vector3D VortSystem::Velocity(Vector3D Pos, Memory& Dipoles, re Eps){
	re Pot,PotX,PotY,PotZ;
	Vector3D PosX,PosY,PosZ;
	
	PosX=Pos+Eps*e1();
	PosY=Pos+Eps*e2();
	PosZ=Pos+Eps*e3();
	
	Pot=Potential(Pos, Dipoles);
	PotX=Potential(PosX,Dipoles);
	PotY=Potential(PosY,Dipoles);
	PotZ=Potential(PosZ,Dipoles);
	
	Vector3D Vel;
	re InvEps=ONE/Eps;
	Vel.v[0]=CapPotDiff(PotX-Pot)*InvEps;
	Vel.v[1]=CapPotDiff(PotY-Pot)*InvEps;
	Vel.v[2]=CapPotDiff(PotZ-Pot)*InvEps;
	
	return Vel;
}

Vector3D VortSystem::VelocityFromVortexLattice(Vector3D Pos, Memory& Dipoles, re CoreSize){
	int Size=Mesh.PolygonStorage.size();
	re InvCoreSize2=1e6;
	
	if(CoreSize>0){
		InvCoreSize2=ONE/(CoreSize*CoreSize);
	}
	
	Vector3D Vel=FreeVelocity(Pos);
	
	for (int i=0; i<Size; i++) {
		re q=DipoleStrength[i];
		Vel=Vel+q*GetSymmetrizedVelocityLatticeInfluence(Mesh.PolygonStorage[i], Pos, InvCoreSize2);
	}
	
	return Vel;
}


Vector3D VortSystem::VelocityAtSingularPoint(Vector3D Pos, Memory& Dipoles, re Eps){
	re PotX0,PotY0,PotZ0,PotX,PotY,PotZ;
	Vector3D PosX0,PosY0,PosZ0,PosX,PosY,PosZ;
	
	PosX0=Pos+Eps*e1();
	PosY0=Pos+Eps*e2();
	PosZ0=Pos+Eps*e3();

	PosX=Pos+TWO*Eps*e1();
	PosY=Pos+TWO*Eps*e2();
	PosZ=Pos+TWO*Eps*e3();
	
	PotX0=Potential(PosX0, Dipoles);
	PotY0=Potential(PosY0, Dipoles);
	PotZ0=Potential(PosZ0, Dipoles);
	PotX=Potential(PosX,Dipoles);
	PotY=Potential(PosY,Dipoles);
	PotZ=Potential(PosZ,Dipoles);
	
	Vector3D Vel;
	re InvEps=ONE/Eps;
	Vel.v[0]=CapPotDiff(PotX-PotX0)*InvEps;
	Vel.v[1]=CapPotDiff(PotY-PotY0)*InvEps;
	Vel.v[2]=CapPotDiff(PotZ-PotZ0)*InvEps;
	
	return Vel;
}


re VortSystem::Potential(Vector3D Pos){
	return Potential(Pos, DipoleStrength);
}

Vector3D VortSystem::Velocity(Vector3D Pos, re Eps){
	return Velocity(Pos, DipoleStrength, Eps);
}


int VortSystem::SolveSystem(){
	AssignSpace();
//	cout<<"Assigned"<<endl;
	BuildMatrix();
//ExportMatrix("TestMatrix.dat");
//	cout<<"Built"<<endl;
	BuildRHS();
	
	DecomposeMatrix();
	
	DipoleStrength=RHS;
	Solve(DipoleStrength);
//	cout<<"Solved"<<endl;
	
	return 0;
}


void VortSystem::ExportMatrix(std::string Name){
	ofstream of(Name.c_str());
	for (int i=0; i<SystemSize; i++) {
		for (int j=0; j<SystemSize; j++) {
			of<<Matrix[MInd(i, j)]<<" ";
		}
		of<<endl;
	}
}

void VortSystem::StreamLineRegular(Vector3D Initial, re timestep, re time, std::vector<Vector3D>& Points, re CoreSize){
	Points.clear();
	
	Vector3D R=Initial;
	
	for (re t=0; t<time; t+=timestep) {
		Points.push_back(R);
		R=R+timestep*VelocityFromVortexLattice(R, DipoleStrength, CoreSize);
	}
	Points.push_back(R);
}

void VortSystem::StreamLine(Vector3D Initial, re timestep, re time, std::vector<Vector3D>& Points){
	Points.clear();
	
	Vector3D R=Initial;
	
	for (re t=0; t<time; t+=timestep) {
		Points.push_back(R);
		R=R+timestep*Velocity(R, DipoleStrength);
	}
	Points.push_back(R);
}

Memory& VortSystem::GetDipolesReference(){
	return DipoleStrength;
}

int VortSystem::ExportClC(std::string FName, Vector3D Up){
	ofstream of(FName.c_str());
	
	if(!of.is_open()){
		return -1;
	}
	
	Vector3D FVUnit=FarVelocity;
	
	Vector3D X;
	TriVectors(&FVUnit, &Up, &X);
	X=-X;
	
	for (EdgeMap::iterator it=Mesh.EdgeStorage.begin(); it!=Mesh.EdgeStorage.end(); it++) {
		VortEdgeInfo& EdgeInf=(it->second);
		VortEdge Edge=(it->first);
		if (EdgeInf.PolygonReferences.size()==3) {
			bool WakeDir;
			int wi=-1;
			int mi;
			VortPolygon* pWake;
			
			for (int i=0; i<3; i++) {
				VortPolygonReference& PolyRef=EdgeInf.PolygonReferences[i];
				if (PolyRef.pPolygon->WakePoly) {
					wi=i;
					mi=PolyRef.Index;
					WakeDir=PolyRef.EdgeDirection;
					pWake=PolyRef.pPolygon;
				}
			}
			assert(wi>=0);
			
			Vector3D P1=Edge.PointRef1.pPoint->Position;
			Vector3D P2=Edge.PointRef2.pPoint->Position;
			
			Vector3D Pos=HALF*(P1+P2);
			Vector3D Diff;
			if (WakeDir) {
				Diff=P2-P1;
			} else {
				Diff=P1-P2;
			}
			
			Vector3D ProjectedDiff=Diff-(FVUnit*Diff)*FVUnit;
			Vector3D ProjectedPos=Pos-(FVUnit*Pos)*FVUnit;
			re SizeDiff=Norm(ProjectedDiff);
			Vector3D LiftDens=DipoleStrength[mi]*Vectorial(FarVelocity, Diff)/SizeDiff;
			
			of<<ProjectedPos*X<<" "<<ProjectedPos*Up<<" "<<LiftDens*X<<" "<<LiftDens*Up<<" "<<SizeDiff<<" "<<DipoleStrength[mi]<<endl;
			
		}
	}
	return 0;
}


int VortSystem::ExportData(std::string FName, Memory& Data){
	ofstream of(FName.c_str());
	
	if(!of.is_open()){
		return -1;
	}
	
	int NPoly=Mesh.PolygonStorage.size();
	for (int i=0; i<NPoly; i++) {
		of<<Data[i]<<" ";
	}
	
	return 0;
	
}


int VortSystem::ExportIsWake(std::string FName){
	ofstream of(FName.c_str());
	
	if(!of.is_open()){
		return -1;
	}
	
	int NPoly=Mesh.PolygonStorage.size();
	for (int i=0; i<NPoly; i++) {
		of<<Mesh.PolygonStorage[i].WakePoly<<" ";
	}
	
	return 0;
	
	
}


int VortSystem::ExportVectors(std::string FName, std::vector<Vector3D>& Data){
	ofstream of(FName.c_str());
	
	if(!of.is_open()){
		return -1;
	}
	
	int NPoly=Mesh.PolygonStorage.size();
	for (int i=0; i<NPoly; i++) {
		of<<Data[i].v[0]<<" "<<Data[i].v[1]<<" "<<Data[i].v[2]<<endl;
	}
	
	return 0;
}


void VortSystem::CalculateDirectPressures(Memory& Pressures){
	int NPoly=Mesh.PolygonStorage.size();
	
	Pressures.assign(NPoly,0);
	Vector3D V,N;
	
	for (int i=0; i<NPoly; i++) {
		N=Mesh.PolygonStorage[i].GetNormal();
		V=Velocity(Mesh.PolygonStorage[i].GetCenterPosition());
		V=V-(V*N)*N;
		Pressures[i]=-HALF*Square(TWO*V);
	}
}



void VortSystem::CalculateDerivativePressures(Memory& Pressures, MemoryVector& Velocities, Vector3D* pForce, Vector3D* pMoment){
	Vector3D F=V0(),M=V0(),dF;
	
	CalculateDerivativeVelocities(Velocities);
	
	Pressures.assign(Mesh.PolygonStorage.size(),0);
	
	for (int i=0; i<Mesh.PolygonStorage.size(); i++) {
		Pressures[i]=-HALF*(Square(Velocities[i])-1);
		VortPolygon& P=Mesh.PolygonStorage[i];
		if (!P.WakePoly) {
			dF=(-Pressures[i]*P.Area)*P.Normal;
			F=F+dF;
			M=M+Vectorial(P.CenterPoint.Position, dF);

		}
	}
	if(pForce!=NULL){
		*pForce=F;
	}
	if(pMoment!=NULL){
		*pMoment=M;
	}
	
}

void VortSystem::CalculateDerivativeVelocities(MemoryVector& Velocities){
	
	Mesh.CalculateMeshDerivativeLMS(DipoleStrength, Velocities);
		
}


void VortSystem::CalculateAbsolutePressureGradient(Memory& Pressures, Memory& Gradients){
	std::vector<Vector3D> Grads;
	
	Mesh.CalculateMeshDerivativeLMS(Pressures, Grads);
	
	Gradients.assign(Mesh.PolygonStorage.size(),0);
	for (int i=0; i<Mesh.PolygonStorage.size(); i++) {
		Gradients[i]=Norm(Grads[i]);
	}	
}

void VortSystem::CalculateDirectedAbsolutePressureGradient(Memory& Pressures, MemoryVector& Velocities, Memory& Gradients){
	std::vector<Vector3D> Grads;

	Mesh.CalculateMeshDerivativeLMS(Pressures, Grads);

	Gradients.assign(Mesh.PolygonStorage.size(),0);
	for (int i=0; i<Mesh.PolygonStorage.size(); i++) {
		Gradients[i]=rabs(Grads[i]*Normalize(Velocities[i]));
	}	
	
}


int VortSystem::RelaxWake(re Factor, re CoreSize){
	VortMesh OtherMesh;
	
	RelaxWakePolys(OtherMesh, FarVelocity, Factor, CoreSize);
	
	Mesh=OtherMesh;
	
	Mesh.Bake();
	
	return 0;
}


int VortSystem::RelaxWakePolys(VortMesh& MovedMesh, Vector3D Direction, re Factor, re CoreSize){
	std::set<int> PointIndexSet;
	int res=0;
	
	Direction=Normalize(Direction);
	
	MovedMesh=Mesh;
	
	for (EdgeMap::iterator it=Mesh.EdgeStorage.begin(); it!=Mesh.EdgeStorage.end(); it++) {
		int iPoly=-1;
		if (it->second.PolygonReferences.size()==3) {
			for (int i=0; i<3; i++) {
				if (it->second.PolygonReferences[i].pPolygon->WakePoly) {
					iPoly=i;
				}
			}
			if (iPoly>=0) {
				VortPolygon& P=*(it->second.PolygonReferences[iPoly].pPolygon);
				VortPolygon& MP=MovedMesh.PolygonStorage[it->second.PolygonReferences[iPoly].Index];
				VortEdge E=it->first;
				VortPointReference StartRef=E.PointRef1;
				if (it->second.PolygonReferences[iPoly].EdgeDirection) {
					StartRef=E.PointRef2;
				} 
				
				int StartIndex=P.GetIndex(StartRef);
				int Increment=1;//!!!!!!!!!!!
				int NPoints=P.PointList.size();
				int NWakeLines=NPoints/2-1;
				for (int WakeLine=0; WakeLine<2; WakeLine++) {
					int EndIndex=(StartIndex+NWakeLines+NPoints)%NPoints;
					for (int Index=StartIndex; Index!=EndIndex; Index=(Index+Increment+NPoints)%NPoints) {
						int NextIndex=(Index+Increment+NPoints)%NPoints;
						if (PointIndexSet.find(P.PointList[NextIndex].Index)==PointIndexSet.end()) {
							PointIndexSet.insert(P.PointList[NextIndex].Index);
						
							Vector3D& M1=MP.PointList[Index].pPoint->Position;
							Vector3D& M2=MP.PointList[NextIndex].pPoint->Position;
							Vector3D& P1=P.PointList[Index].pPoint->Position;
							Vector3D& P2=P.PointList[NextIndex].pPoint->Position;
							
							Vector3D Pos=HALF*(P1+P2);
							Vector3D OldDir=P2-P1;
						
							re D=OldDir*Direction;
							
							Vector3D Vel=Normalize(VelocityFromVortexLattice(Pos, DipoleStrength, CoreSize));
							

							Vel=Factor*Vel+(ONE-Factor)*Normalize(OldDir);
							
							
							
							M2=M1+(D/(Vel*Direction))*Vel;
						}
					}
					StartIndex=(StartIndex+NPoints-1)%NPoints;
					NWakeLines=-NWakeLines;
					Increment=-1;
				}

			} else {
				res--;
			}

		}
	}
	
	return res;
}


